%initialze model
load('results\RoughDist\optimizedGains.mat')
BodyMechParams
ControlParams
prostheticParams

[groundX, groundZ, groundTheta] = generateGround('flat');

model = 'NeuromuscularModelwReflex';
load_system(model)
rtp = Simulink.BlockDiagram.buildRapidAcceleratorTarget(model);

%set gains
rtp = Simulink.BlockDiagram.modifyTunableParameters(rtp, ...
            'LGainGAS',           Gains( 1), ...
            'LGainGLU',           Gains( 2), ...
            'LGainHAM',           Gains( 3), ...
            'LGainKneeOverExt',   Gains( 4), ...
            'LGainSOL',           Gains( 5), ...
            'LGainSOLTA',         Gains( 6), ...
            'LGainTA',            Gains( 7), ...
            'LGainVAS',           Gains( 8), ...
            'LKglu',              Gains( 9), ...
            'LPosGainGG',         Gains(10), ...
            'LSpeedGainGG',       Gains(11), ...
            'LhipDGain',          Gains(12), ...
            'LhipPGain',          Gains(13), ...
            'LkneeExtendGain',    Gains(14), ...
            'LkneeFlexGain',      Gains(15), ...
            'LkneeHoldGain1',     Gains(16), ...
            'LkneeHoldGain2',     Gains(17), ...
            'LkneeStopGain',      Gains(18), ...
            'LlegAngleFilter',    Gains(19), ...
            'LlegLengthClr',      Gains(20), ...
            'RGainGAS',           Gains(21), ...
            'RGainGLU',           Gains(22), ...
            'RGainHAM',           Gains(23), ...
            'RGainHAMCut',        Gains(24), ...
            'RGainKneeOverExt',   Gains(25), ...
            'RGainSOL',           Gains(26), ...
            'RGainSOLTA',         Gains(27), ...
            'RGainTA',            Gains(28), ...
            'RGainVAS',           Gains(29), ...
            'RKglu',              Gains(30), ...
            'RPosGainGG',         Gains(31), ...
            'RSpeedGainGG',       Gains(32), ...
            'RhipDGain',          Gains(33), ...
            'RhipPGain',          Gains(34), ...
            'RkneeExtendGain',    Gains(35), ...
            'RkneeFlexGain',      Gains(36), ...
            'RkneeHoldGain1',     Gains(37), ...
            'RkneeHoldGain2',     Gains(38), ...
            'RkneeStopGain',      Gains(39), ...
            'RlegAngleFilter',    Gains(40), ...
            'RlegLengthClr',      Gains(41), ...
            'simbiconGainD',      Gains(42), ...
            'simbiconGainV',      Gains(43), ...
            'simbiconLegAngle0',  Gains(44), ...
            'anklePgain',         Gains(45), ...
            'ankleDgain',         Gains(46), ...
            'ankleFilterPID',     Gains(47), ...
            'ankleFilterSEA',     Gains(48), ...
            'kneePgain',          Gains(49), ...
            'kneeDgain',          Gains(50), ...
            'kneeFilterPID',      Gains(51), ...
            'kneeFilterSEA',      Gains(52), ...
            'legAngleTgt',        Gains(53));

numSamples = 50;
terrains = 0:0.02:.14;
%terrains = 0.08;
numTerrains = length(terrains);
totalRuns = numSamples*numTerrains;
dists = nan(totalRuns,1);
paramSets = cell(1,totalRuns);
idx = 1;
for i = 1:numTerrains
    for j = 1:numSamples
        [groundX, groundZ, groundTheta] = generateGround('const',terrains(i),idx);

        paramSets{idx} = Simulink.BlockDiagram.modifyTunableParameters(rtp, ...
            'groundZ',     groundZ, ...
            'groundTheta', groundTheta);
        idx = idx+1;
    end
end

parfor i = 1:length(paramSets)
    dists(i) = evaluateCostParallel(paramSets{i});
end

dists = reshape(dists,numSamples,[])
mediandists = nanmedian(dists)

save('distances.mat','dists')
